#!/usr/bin/env python3    
# -*- coding:UTF-8 -*-
import sys, rospy, cv2, time
import math
#import numpy as np

from geometry_msgs.msg import Twist
from position_msg.msg import target,Point
from std_msgs.msg import String
from std_msgs.msg import Float64

adjust = 0
feedback_value=[0.32,0.18,0.50]

class PIDController:

	def __init__(self, Kp, Ki, Kd):
		self.move_cmd = Twist()
		self.Kp = Kp
		self.Ki = Ki
		self.Kd = Kd
		self.clear()
		self.cmd_vel = rospy.Publisher('cmd_vel_mux/input/navi', Twist, queue_size=10)


	def clear(self):
		self.SetPoint = [0.0] * 3  
		self.PTerm = [0.0] * 3
		self.ITerm = [0.0] * 3
		self.DTerm = [0.0] * 3
		self.last_error = [0.0] * 3

	def update(self, feedback_value):

		error = [sp - fb for sp, fb in zip(self.SetPoint, feedback_value)]

		self.PTerm = [self.Kp * err for err in error]
		self.ITerm = [self.ITerm[i] + err for i, err in enumerate(error)]
		self.DTerm = [err - self.last_error[i] for i, err in enumerate(error)]

		self.last_error = error
		control_signal = [self.Kp * err + self.Ki * self.ITerm[i] + self.Kd * self.DTerm[i] for i, err in enumerate(error)]
		if feedback_value[2]==-1:
			control_signal[2]=0
		return control_signal
		

	def registeration(self,control_signal,a):
		#newpoint=[self.SetPoint[i]+control_signal[i] for i in range(3) ]
		global feedback_value
		if control_signal[2]<0.5 and a==1 and feedback_value[2]!=0 : #and control_signal[0]<:
			theta=control_signal[0]/feedback_value[2]
			self.move_cmd.angular.z = 5*math.atan(theta)
			if self.move_cmd.angular.z>2:#不要冲太快
				self.move_cmd.angular.z=2
			self.cmd_vel.publish(self.move_cmd)
			print(self.move_cmd.angular.z)
			print(1111)
		elif control_signal[2]<0 and a==2:
			self.move_cmd.linear.x = -0.8*control_signal[2]
			self.move_cmd.angular.z = 0
			if self.move_cmd.linear.x>0.5:#不要冲太快
				self.move_cmd.linear.x=0.5
			self.cmd_vel.publish(self.move_cmd)
			print(self.move_cmd.linear.x)
			print(2222)

		elif a==3:

			self.move_cmd.linear.x = 0.2
			self.move_cmd.angular.z = 0
			self.cmd_vel.publish(self.move_cmd)
			rospy.sleep(0.3)
			self.cmd_vel.publish(self.move_cmd)
			rospy.sleep(0.3)
			self.cmd_vel.publish(self.move_cmd)
			rospy.sleep(0.3)
			self.cmd_vel.publish(self.move_cmd)
			rospy.sleep(0.3)
			print(3333)						
			#self.move_cmd.linear.x = 0
			#self.move_cmd.angular.z = 0
			#self.cmd_vel.publish(self.move_cmd)

		elif control_signal[2]<0.5 and a==4: #and control_signal[0]<:
			theta=control_signal[0]/feedback_value[2]
			self.move_cmd.angular.z = 2*math.atan(theta)
			if self.move_cmd.angular.z>2:#不要冲太快
				self.move_cmd.angular.z=2
			self.cmd_vel.publish(self.move_cmd)
			print(self.move_cmd.angular.z)
			print(4444)
		rospy.sleep(0.4)

def begin(data):
	global pid_controller
	global adjust
	global feedback_value
	if data.data=='adjust' and adjust == 0:
		#print(1)
		pid_controller.SetPoint=[0.32,0.18,0.50]#111
		i=1
		
		while (1) :#feedback_value[2]<0.395 or feedback_value[2]>0.405:
			i=i+1
			control_signal=pid_controller.update(feedback_value)
			print(control_signal)
			pid_controller.registeration(control_signal=control_signal,a=1)
			if i>30:
				break
		while(1):
			i=i+1
			control_signal=pid_controller.update(feedback_value)
			print(control_signal)
			pid_controller.registeration(control_signal=control_signal,a=2)
			if i>70:
				break

		while (1) :#feedback_value[2]<0.395 or feedback_value[2]>0.405:
			i=i+1
			control_signal=pid_controller.update(feedback_value)
			print(control_signal)
			pid_controller.registeration(control_signal=control_signal,a=4)#最后一次调整角度慢一些
			if i>90:
				break
		
		#pid_controller.registeration(control_signal=[0,0,0],a=3)
		adjust = 1
		pub_pid = rospy.Publisher('PID_back', String, queue_size=10)
		print("pid结束")
		pub_pid.publish("adjust_finish")

	
def get_point(data):
	global feedback_value
	feedback_value[0]=data.point.x*0.001
	feedback_value[1]=data.point.y*0.001
	feedback_value[2]=data.point.z*0.001

def talk():
	global adjust
	pub=rospy.Publisher("PID_back",String,queue_size=10)
	# count=0
	while not rospy.is_shutdown():
		if adjust==1:
			#print("finish1")
			# count=count+1
			pub.publish("adjust_finish")
			break
			# if count==3:
			# 	break
			

def listen():
	rospy.Subscriber("pid",String,begin)
	rospy.Subscriber("/bottle_coordinates",target,get_point)
	
pid_controller=PIDController(Kp=0.6,Ki=0,Kd=0.001)

if  __name__=='__main__':
	rospy.init_node('pid')
	listen()
	talk()



